#include "gm.h"
#include "gmlan.h"

#define GM_PID_INTERVAL         10

const GmPidMap mGmPidMap[ENG_DATA_SIZE] =
{
    { ENG_DATA_RPM,           0x00, 0x0c, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 2, SUPPORTED },
    { ENG_DATA_VS,            0x00, 0x0d, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_ECT,           0x00, 0x05, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_IAT,           0x00, 0x0f, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_APP,           0x13, 0x1f, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_TP,            0x12, 0xb1, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_ERT,           0x00, 0x1f, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 2, SUPPORTED },
    { ENG_DATA_LOAD,          0x00, 0x04, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_BATTVOLT,      0x00, 0x04, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, UNSUPPORTED },
    { ENG_DATA_REALFUELCO,    0x00, 0x04, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, UNSUPPORTED },
    { ENG_DATA_FUELLEVEL,     0x13, 0x2a, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 2, SUPPORTED },
    { ENG_DATA_ODO,           0x00, 0x04, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, UNSUPPORTED },
    { ENG_DATA_LTFT,          0x00, 0x07, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_STFT,          0x00, 0x06, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_MISFIRE1,      0x12, 0x06, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_MISFIRE2,      0x12, 0x05, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_MISFIRE3,      0x12, 0x07, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_MISFIRE4,      0x12, 0x08, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_MISFIRE5,      0x11, 0xea, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_MISFIRE6,      0x11, 0xeb, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_FCLS,          0x11, 0x31, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_KEYSTATUS,     0x20, 0x7e, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_HO2S1,         0x23, 0x20, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_HO2S2,         0x23, 0x21, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_MAP,           0x20, 0x8a, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_INJECTPULSE,   0x12, 0xc3, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 2, SUPPORTED },
    { ENG_DATA_OILPRESSURE,   0x11, 0x27, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, SUPPORTED },
    { ENG_DATA_OILLEVELSTATUS,0x00, 0x04, GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE, 1, 1, UNSUPPORTED },
};

const GmPidMap mGmBcmPidMap[BCM_DATA_SIZE] =
{
    { BCM_DATA_LFWIN,          0x00, 0x0c, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 2, UNSUPPORTED },
    { BCM_DATA_RFWIN,          0x00, 0x0d, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, UNSUPPORTED },
    { BCM_DATA_LRWIN,          0x00, 0x05, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, UNSUPPORTED },
    { BCM_DATA_RRWIN,          0x00, 0x0f, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, UNSUPPORTED },
    { BCM_DATA_DDA,            0x80, 0x85, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, SUPPORTED },
    { BCM_DATA_PDA,            0x80, 0x86, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, SUPPORTED },
    { BCM_DATA_RRDA,           0x80, 0x88, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, SUPPORTED },
    { BCM_DATA_LRDA,           0x80, 0x87, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, SUPPORTED },
    { BCM_DATA_SUNROOF,        0x90, 0xe0, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 1, SUPPORTED },
    { BCM_DATA_PARKLAMP,       0x80, 0x4c, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 2, SUPPORTED },
    { BCM_DATA_HEADLAMP,       0x80, 0x4c, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 2, SUPPORTED },
    { BCM_DATA_CHARGESTATUS,   0x90, 0xf8, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 2, SUPPORTED },
    { BCM_DATA_BATTCURRENT,    0x90, 0xfb, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 2, SUPPORTED },
    { BCM_DATA_BATTSTATUS,     0x09, 0x00, GMLAN_SERVICE_ID_READ_STATIC_DATA,  0, 2, SUPPORTED },
    { BCM_DATA_BATTVOLT,       0x90, 0xb1, GMLAN_SERVICE_ID_MODE_TWENTY_TWO,   0, 2, SUPPORTED },
    { BCM_DATA_ODO,            0xdf, 0x00, GMLAN_SERVICE_ID_READ_STATIC_DATA,  1, 4, SUPPORTED },
};

VehicleCtrlOps mGmCtrlOps;
VehicleProcessOps mGmProcessOps;
uint8_t mGmEngineOn = TRUE;

void gm_init(Vehicle *vehicle)
{
    logi("%s", __func__);
    mGmCtrlOps.support |= SUPPORT_CONTROL_WINDOW;
    mGmCtrlOps.support |= SUPPORT_CONTROL_DOOR;
    mGmCtrlOps.support |= SUPPORT_CONTROL_LIGHT;
    mGmCtrlOps.support |= SUPPORT_CONTROL_SUNROOF;
    mGmCtrlOps.support |= SUPPORT_CONTROL_TRUNK;
    mGmCtrlOps.support |= SUPPORT_CONTROL_FINDCAR;

    mGmCtrlOps.control_window = gm_control_window;
    mGmCtrlOps.control_door = gm_control_door;
    mGmCtrlOps.control_light = gm_control_light;
    mGmCtrlOps.control_sunroof = gm_control_sunroof;
    mGmCtrlOps.control_trunk = gm_control_trunk;
    mGmCtrlOps.control_findcar = gm_control_findcar;

    mGmProcessOps.check_engine_on = gm_check_engine_on;
    mGmProcessOps.get_rpm = gm_get_rpm;
    mGmProcessOps.process_eng_data = gm_process_eng_data;
    mGmProcessOps.process_bcm_data = gm_process_bcm_data;
    mGmProcessOps.get_fault_code = gm_get_fault_code;

    vehicle->ctrlOps = &mGmCtrlOps;
    vehicle->processOps = &mGmProcessOps;
    bsp_can_filter(0x7e8, 0x7e8, 0x7ff, 0x7ff);
}

void gm_control_window(uint8_t state)
{
    CanTxMsg window_on =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x3b, 0x0f, 0x03, 0x03, 0x03, 0x03
    };
    CanTxMsg window_off =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x3b, 0x0f, 0x02, 0x02, 0x02, 0x02
    };

    logi("%s: state = %d", __func__, state);
    if(state) {
        bsp_can_send(&window_on);
        os_delay(5);
        bsp_can_send(&window_on);
        os_delay(5);
    } else {
        bsp_can_send(&window_off);
        os_delay(5);
        bsp_can_send(&window_off);
        os_delay(5);
    }
    bsp_can_clear();
}

void gm_control_door(uint8_t state)
{
    CanTxMsg door_temp =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00
    };

    logi("%s: state = %d", __func__, state);
    if(state) {
        door_temp.Data[3] = 0x04;
        door_temp.Data[4] = 0x04;
        bsp_can_send(&door_temp);
        door_temp.Data[3] = 0x04;
        door_temp.Data[4] = 0x00;
        bsp_can_send(&door_temp);
        door_temp.Data[3] = 0x02;
        door_temp.Data[4] = 0x02;
        bsp_can_send(&door_temp);
        door_temp.Data[3] = 0x02;
        door_temp.Data[4] = 0x00;
        bsp_can_send(&door_temp);
        door_temp.Data[0] = 0x02;
        door_temp.Data[3] = 0x00;
        door_temp.Data[4] = 0x00;
        bsp_can_send(&door_temp);
        os_delay_ms(500);
    } else {
        door_temp.Data[3] = 0x01;
        door_temp.Data[4] = 0x01;
        bsp_can_send(&door_temp);
        door_temp.Data[3] = 0x01;
        door_temp.Data[4] = 0x00;
        bsp_can_send(&door_temp);
        door_temp.Data[3] = 0x01;
        door_temp.Data[4] = 0x01;
        bsp_can_send(&door_temp);
        door_temp.Data[3] = 0x01;
        door_temp.Data[4] = 0x00;
        bsp_can_send(&door_temp);
        door_temp.Data[0] = 0x02;
        door_temp.Data[3] = 0x00;
        door_temp.Data[4] = 0x00;
        bsp_can_send(&door_temp);
        os_delay_ms(500);
    }
    bsp_can_clear();
}

void gm_control_light(uint8_t state)
{
    CanTxMsg light_temp =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    logi("%s: state = %d", __func__, state);
    if(state) {
        light_temp.Data[5] = 0x02;
        light_temp.Data[6] = 0x02;
        bsp_can_send(&light_temp);
        os_delay_ms(500);
    } else {
        light_temp.Data[5] = 0x02;
        light_temp.Data[6] = 0x00;
        bsp_can_send(&light_temp);
        os_delay_ms(500);
    }
    bsp_can_clear();
}
void gm_control_sunroof(uint8_t state)
{
    CanTxMsg on =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x4c, 0xff, 0x08, 0x00, 0x00, 0x00
    };
    CanTxMsg off =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x4c, 0xff, 0x04, 0x00, 0x00, 0x00
    };
    logi("%s: state = %d", __func__, state);
    if(state) {
        bsp_can_send(&on);
        os_delay(3);
        bsp_can_send(&on);
        os_delay(3);
        bsp_can_send(&on);
        os_delay(3);
    } else {
        bsp_can_send(&off);
        os_delay(3);
        bsp_can_send(&off);
        os_delay(3);
        bsp_can_send(&off);
        os_delay(3);
    }
    bsp_can_clear();
}
void gm_control_trunk(uint8_t state)
{
    CanTxMsg on =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x01, 0x10, 0x10, 0x00, 0x00, 0x00
    };

    logi("%s: state = %d", __func__, state);
    if(state) {
        bsp_can_send(&on);
        os_delay_ms(500);
        on.Data[4] = 0x00;
        bsp_can_send(&on);
        os_delay_ms(500);
    }
    bsp_can_clear();
}
void gm_control_findcar(uint8_t state)
{
    CanTxMsg on =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x07, 0xae, 0x10, 0x01, 0x01, 0x00, 0x00, 0x00
    };
    CanTxMsg off =
    {
        0x241, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        8,
        0x02, 0xae, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
    };
    logi("%s: state = %d", __func__, state);
    if(state) {
        bsp_can_send(&on);
        os_delay_ms(500);
        on.Data[2] = 0x02;
        on.Data[3] = 0xf0;
        on.Data[4] = 0xf0;
        bsp_can_send(&on);
        os_delay_ms(500);
    } else {
        bsp_can_send(&off);
        os_delay_ms(500);
    }
    bsp_can_clear();
}

uint8_t gm_check_engine_on(void)
{
    return mGmEngineOn;
}

void gm_get_rpm(uint16_t *rpm)
{
    gmlan_data *data;
    gmlan_recv recv;

    data = (gmlan_data *)malloc(sizeof(gmlan_data));
    memset(data, 0x00, sizeof(gmlan_data));
    data->tx_id = 0x7e0;
    data->rx_id = 0x7e8;
    data->rx_id2 = 0x5e8;
    data->len = 0x04;
    data->cmd[0] = 0x00;
    data->cmd[1] = 0x0c;
    data->service_id = GMLAN_SERVICE_ID_DYNAMIC_PID_DEFINE;
    //TODO
    data->index = 0xFE;

    if(gmlan_ioctl(data, &recv)) {
        *rpm = (recv.data[1] << 8) | recv.data[2];
    } else {
        *rpm = 0;
    }
    free(data);
}

void gm_process_eng_data(EngineDataMap *dataMap)
{
    uint8_t i;
    uint8_t support;
    uint8_t pid;
    uint8_t offset;
    uint8_t size;
    gmlan_data *data;
    gmlan_recv recv;

    logi("%s", __func__);
    for(i = 0; i < ENG_DATA_SIZE; i++) {
        os_delay_ms(200);
        vehicle_lock();
        //send keepalive
        support = mGmPidMap[i].support;
        //check wether this id is supported?
        if(support == UNSUPPORTED) {
            vehicle_unlock();
            continue;
        }
        if(gm_keepalive(0x7e0, 0x7e8) == FALSE) {
            vehicle_unlock();
            continue;
        }
        pid = mGmPidMap[i].id;
        offset = mGmPidMap[i].offset;
        size = mGmPidMap[i].size;

        data = (gmlan_data *)malloc(sizeof(gmlan_data));
        memset(data, 0x00, sizeof(gmlan_data));
        data->tx_id = 0x7e0;
        data->rx_id = 0x7e8;
        data->rx_id2 = 0x5e8;
        data->len = 0x04;
        data->cmd[0] = mGmPidMap[i].cmd[0];
        data->cmd[1] = mGmPidMap[i].cmd[1];
        data->service_id = mGmPidMap[i].service_id;
        //TODO
        data->index = 0xFE;

        if(gmlan_ioctl(data, &recv)) {
            dataMap->data[i].id = i;
            dataMap->data[i].updated = TRUE;
            dataMap->data[i].len = size;
            memcpy(dataMap->data[i].data, recv.data + recv.offset + offset, size);
            if(i == ENG_DATA_RPM) {
                mGmEngineOn = TRUE;
            }
        } else {
            if(i == ENG_DATA_RPM) {
                mGmEngineOn = FALSE;
            }
        }
        free(data);
        vehicle_unlock();
        os_delay(GM_PID_INTERVAL);
    }
}

void gm_process_bcm_data(BcmDataMap *dataMap)
{
    uint8_t i;
    uint8_t support;
    uint8_t pid;
    uint8_t offset;
    uint8_t size;
    gmlan_data *data;
    gmlan_recv recv;

    logi("%s", __func__);
    for(i = 0; i < BCM_DATA_SIZE; i++) {
        os_delay_ms(200);
        vehicle_lock();
        //send keepalive
        support = mGmBcmPidMap[i].support;
        //check wether this id is supported?
        if(support == UNSUPPORTED) {
            vehicle_unlock();
            continue;
        }
        if(gm_keepalive(0x241, 0x641) == FALSE) {
            vehicle_unlock();
            break;
        }
        pid = mGmBcmPidMap[i].id;
        offset = mGmBcmPidMap[i].offset;
        size = mGmBcmPidMap[i].size;

        data = (gmlan_data *)malloc(sizeof(gmlan_data));
        memset(data, 0x00, sizeof(gmlan_data));
        data->tx_id = 0x241;
        data->rx_id = 0x641;
        if(mGmBcmPidMap[i].service_id == GMLAN_SERVICE_ID_READ_STATIC_DATA) {
            data->len = 0x02;
        } else if(mGmBcmPidMap[i].service_id == GMLAN_SERVICE_ID_MODE_TWENTY_TWO) {
            data->len = 0x03;
        }
        data->cmd[0] = mGmBcmPidMap[i].cmd[0];
        data->cmd[1] = mGmBcmPidMap[i].cmd[1];
        data->service_id = mGmBcmPidMap[i].service_id;

        if(gmlan_ioctl(data, &recv)) {
            dataMap->data[i].id = i;
            dataMap->data[i].updated = TRUE;
            dataMap->data[i].len = size;
            memcpy(dataMap->data[i].data, recv.data + recv.offset + offset, size);
        } else {
            free(data);
            vehicle_unlock();
            break;
        }
        free(data);
        vehicle_unlock();
        os_delay(GM_PID_INTERVAL);
    }
}

void gm_get_fault_code(uint8_t type, FaultCodeItem *item)
{
    uint8_t ret;
    gmlan_codes_result result;
    gmlan_codes_cmd codes_cmd[CODE_SIZE] =
    {
        //ENG CODE
        {0x7e0, 0x5e8, 0x03, 0xa9, 0x81, 0x1a},
        //AT CODE
        {0x7e2, 0x5ea, 0x03, 0xa9, 0x81, 0x1a},
        //ABS CODE
        {0x243, 0x543, 0x03, 0xa9, 0x81, 0x12},
        //SRS CODE
        {0x247, 0x547, 0x03, 0xa9, 0x81, 0x12},
        //BCM CODE
        {0x241, 0x541, 0x03, 0xa9, 0x81, 0x12},
        //IPC CODE
        {0x24c, 0x54c, 0x03, 0xa9, 0x81, 0x12},
        //EPS CODE
        {0x242, 0x542, 0x03, 0xa9, 0x81, 0x12},
        //AC CODE
        {0x251, 0x551, 0x03, 0xa9, 0x81, 0x12},
        //TPMS CODE
        {0x00},
    };
    result.id = type;
    ret = gmlan_get_codes(&codes_cmd[type], &result);

    if(ret == TRUE) {
        item->type = type;
        item->count = result.count;
        memcpy(item->codes, result.codes, sizeof(uint16_t) * (result.count));
    }
}

uint8_t gm_keepalive(uint16_t tx_id, uint16_t rx_id)
{
    uint8_t ret = TRUE;
    CanRxMsg rxMsg;
    CanTxMsg txMsg =
    {
        0x0000, 0x18db33f1,
        CAN_ID_STD, CAN_RTR_DATA,
        2,
        0x01, 0x3e
    };

    txMsg.StdId = tx_id;
    bsp_can_clear();
    bsp_can_filter(rx_id, rx_id, rx_id | 0x0ff, rx_id | 0x0ff);
    if(vehicle_can_session(&txMsg, &rxMsg)) {
        ret = TRUE;
    } else {
        loge("keepalive error tx[%04x] rx[%04x]", tx_id, rx_id);
        ret = FALSE;
    }
    return ret;
}

